#pragma once

// Copyright (c) 2010 Manuel Peinado Gallego <manuel.peinado@gmail.com>
// Distributed under the MIT License

// Mat4.h : interface of the Mat4class
//

#include "alg3d/Mat3.h"
#include "alg3d/Vec4.h"
#include <iosfwd>

namespace gx
{
    // --------------------------------------------------------------
    // "Mat4" class
    // --------------------------------------------------------------
    // Represents a SRT (scaling+rotation+translation) matrix. 
    // We obviously continue with column vectors convention of the
    // Mat3 class. This means that the translation part occupies the
    // 4th column of the matrix, and that the 4th row is always 
    // 0,0,0,1. We store the matrix as a SR 3x3 submatrix (an 
    // instance of Mat3) and a 3D vector (Vec3 instance) that 
    // represents the the translation part. The whole matrix is
    // then defined as SRT = SR * T(u) where u is the translation
    // vector and T(u) is simply 
    //       /          \
    // T(u)= |I_3x3 | u |
    //       |0 0 0 | 1 |
    //       \          /
    // That product yields simply:
    //       /          \
    // SRT = | SR   | u |
    //       |0 0 0 | 1 |
    //       \          /
    // --------------------------------------------------------------

    class Mat4
    {
    public:
        // ----------------------------------------------------------
        // Member variables. We make them public because the 
        // necessary protection is already provided by the member 
        // functions of "Mat3"
        // ----------------------------------------------------------

        Mat3 rotation;
        Vec3 translation;

        // ----------------------------------------------------------
        // Create an unitialized matrix
        // ----------------------------------------------------------

        Mat4(); 

        // ----------------------------------------------------------
        // Create an identity matrix. Example of use:
        //   gx::Mat4 m(gx::eye);
        // ----------------------------------------------------------

        explicit Mat4(Eye);

        // ----------------------------------------------------------
        // Create a matrix from its individual components. The 
        // arguments mij, i,j<3 should represent a valid SR matrix.
        // The fourth row is not passed as it's always 0,0,0,1
        // ----------------------------------------------------------

        Mat4(double m00, double m01, double m02, double m03,
             double m10, double m11, double m12, double m13,
             double m20, double m21, double m22, double m23);

        // ----------------------------------------------------------
        // Create a matrix from its individual components. The 
        // columns col_i, i<3 should represent a valid SR matrix.
        // The fourth row is not passed as it's always 0,0,0,1
        // ----------------------------------------------------------
        Mat4(const Vec3 & col0, const Vec3 & col1, 
             const Vec3 & col2, const Vec3 & col3=gx::zero);

        // ----------------------------------------------------------
        // Access individual colums from a matrix
        // ----------------------------------------------------------

        Vec3& get_col_0();
        Vec3& get_col_1();
        Vec3& get_col_2();
        Vec3& get_col_3();

        Vec3 get_col_0() const;
        Vec3 get_col_1() const;
        Vec3 get_col_2() const;
        Vec3 get_col_3() const;

        // ----------------------------------------------------------
        // Indexed acces to columns. This is somewhat slow, so 
        // get_col_x should be used when possible
        // ----------------------------------------------------------

        Vec3& get_col(int i);
        Vec3 get_col(int i) const;

        // ----------------------------------------------------------
        // Compares two matrices for equality
        // ----------------------------------------------------------

        bool equals(const Mat4 & other, Real tol = 1e-8) const;

        // ----------------------------------------------------------
        // Copies the components of this vector to a 2D array. 
        // Typical usage:
        //   Real a[4][4];
        //   m4.to_2d_array(a);
        //   cout << "the translation part is: " << Vec3(a[2]);
        // Note: the memory layout of a Mat4 does not match that of
        // a 4x4 C array, so a somewhat expensive conversion must be
        // made
        // ----------------------------------------------------------

        typedef Real Real4[4];
        Real4 * to_2d_array(Real4 * m) const;

        // ----------------------------------------------------------
        // Copies the components of this vector to a 1D array. 
        // Typical usage:
        //   Real a[16];
        //   glMultMatrixd(a); // assumes Real=double
        // Note: the memory layout of a Mat4 does not match that of
        // a 16-length C array, so a somewhat expensive conversion 
        // must be made
        // ----------------------------------------------------------
        Real * to_array(Real m[16]) const;

        // ----------------------------------------------------------
        // Members that modify this matrix 
        // ----------------------------------------------------------

        Mat4 & invert_affine();
        Mat4 & invert();
        
        // ----------------------------------------------------------
        // Functions that compose two transformation matrices
        // ----------------------------------------------------------

        friend Mat4 operator*(const Mat4 & a, const Mat4 & b);
        Mat4 & concat(const Mat4 & second);

        // ----------------------------------------------------------
        // Turn a matrix into a given transformation matrix. They all
        // modify an existing object. You can use the no-init 
        // constructor to obtain a new matrix: 
        //     Mat4().make_rot(q);
        // To obtain a SRT transformation in one statement:
        //     Mat4().make_transl(u) * 
        //       (Mat3().make_rot(q) * Mat3().make_scale(.5));
        // You can write a helper function if you find this ugly:
        //    Mat4 m = make_SRT(s, q, t);
        // Note: when dealing with pure SR transformation, use the
        // Mat3 versions of make_rot if possible. For instance, in
        // the example above we have used Mat3::make_rot and 
        // Mat3::make_scale and then used the Mat4*Mat3 overloaded
        // operator to perform the final promotion to Mat4
        // ----------------------------------------------------------

        Mat4 & make_transl(double tx, double ty, double tz);
        Mat4 & make_transl(const Vec3 & translation);
        
        Mat4 & make_rot(const gx::Mat3 & rotation);
        Mat4 & make_rot(const gx::AxisAngle & aa);
        Mat4 & make_pivot_rot(const Vec3 & pivot, const Mat3 & rot);

        Mat4 & make_scale(double s);
        Mat4 & make_scale(const Vec3 & s);
        Mat4 & make_pivot_scale(const Vec3 & pivot, double s);
        Mat4 & make_pivot_scale(const Vec3 & pivot, const Vec3 & s);
    };

    // --------------------------------------------------------------
    // Free functions that compose a Mat3 and a Mat4, treating 
    // the Mat3 as a "translation=0" Mat4
    // --------------------------------------------------------------

    Mat4 operator*(const Mat4 & m4, const Mat3 & m3);
    Mat4 operator*(const Mat3 & m3, const Mat4 & m4);


    // --------------------------------------------------------------
    // Free function that transforms an homogeneous vector. Note that 
    // no function to transform a Vec3 is provided. This is deliberate, 
    // because the operation "transform a Vec3" is ambiguous: the 
    // result will depend on whether the Vec3 represents a direction 
    // or a point. This is made explicit in the promotion from Vec3 
    // to Vec4. For example:
    //    Vec3 v = whatever;
    //    Vec3 w = m4 * v; // Compiler error prevents ambiguity
    //    Vec3 w = m4 * Vec4(v, 1.); // OK, transforms as point
    // Note that the * operator returns a Vec4, but we are assigning 
    // it to a Vec3. This is possible because Vec3 has a non-explicit 
    // constructor taking a Vec3
    // --------------------------------------------------------------
    
    Vec4 operator*(const Mat4 & a, const Vec4 & v);

    // ----------------------------------------------------------
    // I/O functions to perform IO with a Mat4
    // ----------------------------------------------------------
    
    std::ostream &operator<<(std::ostream &os, const Mat4 & val);
    std::istream &operator>>(std::istream &is, Mat4 & val);

    // --------------------------------------------------------------
    // Implementation of some member functions whose 
    // speed we deem critical
    // --------------------------------------------------------------

    inline Mat4::Mat4() 
    {
    }

    inline Mat4::Mat4(Eye const) 
    : rotation(gx::eye),
      translation(zero)
    {
    }

    inline Mat4::Mat4(double m00, double m01, double m02, double m03, 
                      double m10, double m11, double m12, double m13,
                      double m20, double m21, double m22, double m23)
    : rotation(m00, m01, m02, m10, m11, m12, m20, m21, m22),
      translation(m03, m13, m23)
    { 
    }

    inline Mat4::Mat4(const Vec3 & col0, const Vec3 & col1, 
               const Vec3 & col2, const Vec3 & col3) 
    : rotation(col0, col1, col2), 
      translation(col3)
    { 
    }

    inline bool Mat4::equals(const Mat4 & other, Real tol) const
    {
        return rotation.equals(other.rotation, tol) 
            && translation.equals(other.translation, tol);
    }

    inline Vec4 operator*(const Mat4 & a, const Vec4 & v)
    {
        return Vec4(a.rotation * v.asVec3() 
                    + a.translation * v.w, v.w);
    }

    inline Vec3 Mat4::get_col_0() const
    {
        return rotation.col_0;
    }

    inline Vec3 Mat4::get_col_1() const
    {
        return rotation.col_1;
    }

    inline Vec3 Mat4::get_col_2() const
    {
        return rotation.col_2;
    }

    inline Vec3 Mat4::get_col_3() const
    {
        return translation;
    }

    inline Vec3& Mat4::get_col_0()
    {
        return rotation.col_0;
    }

    inline Vec3& Mat4::get_col_1()
    {
        return rotation.col_1;
    }

    inline Vec3& Mat4::get_col_2()
    {
        return rotation.col_2;
    }

    inline Vec3& Mat4::get_col_3()
    {
        return translation;
    }

    inline Vec3 Mat4::get_col(int i) const 
    {   
        if(i == 0)
        {
            return rotation.col_0;
        }
        if(i == 1)
        {
            return rotation.col_1;
        }
        if(i == 2)
        {
            return rotation.col_2;
        }
        if(i == 3)
        {
            return translation;
        }
        throw std::exception();
    }

    inline Vec3& Mat4::get_col(int i)
    {   
        if(i == 0)
        {
            return rotation.col_0;
        }
        if(i == 1)
        {
            return rotation.col_1;
        }
        if(i == 2)
        {
            return rotation.col_2;
        }
        if(i == 3)
        {
            return translation;
        }
        throw std::exception();
    }
}

